#!/usr/bin/python3
from xgolib import XGO  
import rospy
from geometry_msgs.msg import Twist
import math

def cmd_vel_callbck(msg):
    global dog
    x_speed = msg.linear.x * 100
    y_speed = msg.linear.y * 75
    yaw = msg.angular.z * 180 / math.pi
    dog.move('x', x_speed)
    dog.move('y', y_speed)
    dog.turn(yaw)
    rospy.loginfo("x:%d, y:%d", x_speed, y_speed)



if __name__ == '__main__':
    dog = XGO("xgolite")
    dog.move('x', 0)
    dog.move('y', 0)
    dog.turn(0)
    rospy.init_node('dog_move', anonymous=True)
    rospy.Subscriber('/cmd_vel', Twist, cmd_vel_callbck, queue_size=10)
    rospy.loginfo("Start")
    rospy.spin()










